﻿using System.Collections;
using System.Collections.Generic;
using System.Threading;
using System.Reflection;
using UnityEngine;
using WebSocketSharp;
using WebSocketSharp.Net;
using WebSocketSharp.Server;
using ROSBridgeLib;
using System;
using SimpleJSON;
using UnityEngine.UI;
using System.Net;
using System.Net.Sockets;
using System.Text;
using UnityEngine.Events;


public class BaseLaunch : MonoBehaviour
{

	
    public string address;

    public int websocketport;

    public int udpPort;

    public string launchName;

    private bool autoReconnect = false;

    public float reconnectDelay = 2.5f;

    public BaseSubscriber[] subscribers;

    public BasePublisher[] publishers;
   
    private Hashtable incomingMessages;

    private List<Thread> parseThreads;

    private List<JSONNode> parsedMessages;

    private Hashtable minParseInterval;

    private Hashtable performanceInfo;

    private bool reConnecting;

    public bool connected;

    private bool lastConnected = false;

    Thread receiveThread;


    [SerializeField]
    UnityEvent whenConnected;

    [SerializeField]
    UnityEvent whenDisconnected;

    [SerializeField]
    Text performance;
 


    WebSocket websocket;

    UdpClient udpsocket;

    IPEndPoint remoteEP = null;


    void Start()
    {
        
      

    }


    public void OnRobotProcessChange(string process, bool live)
    {
        
        if (process.Equals(launchName))
        {
            
            autoReconnect=live;

            if (live==true)
            {
                MessageBox.ShowMessage("机器人功能启动");
            }else
            {
                MessageBox.ShowMessage("机器人功能断开");
            }

            OnRobotLaunchStatusChange(live);


           
            Debug.Log("found process:"+process+"is "+ live);

        }
            

    }


    public virtual void OnRobotLaunchStatusChange(bool isLive)
    {

       


    }

   

    void OnEnable()
    {
        
        connected = false;

        InvokeRepeating("PrintPerformanceInfo", 0.5f, 0.5f);

        InvokeRepeating("Reconnect", reconnectDelay, reconnectDelay);

        OnConnectChange(false);

        MessageBox.ShowMessage("等待机器人功能启动...");

    }


    void Reconnect()
    {

        if (autoReconnect == true && connected == false)
        {

            Connect();

        }

    }



    void OnDisable()
    {


        CancelInvoke();

        Clean();

        OnConnectChange(false);


    }

    void Clean()
    {



       

        if (parseThreads != null)
        {

            foreach (Thread thread in parseThreads)
            {
            
                thread.Abort();

            }
        }


        if (receiveThread != null)
        {
            receiveThread.Abort();
        }
      

        if (incomingMessages != null)
        {

            incomingMessages.Clear();
        }

        if (parseThreads != null)
        {
            parseThreads.Clear();
        }

        if (parsedMessages != null)
        {
            parsedMessages.Clear();
        }

        if (performanceInfo != null)
        {
            performanceInfo.Clear();
        }
     


        foreach (BaseSubscriber sub in subscribers)
        {

            sub.OnReset();


        }


    }



    public void PrintPerformanceInfo()
    {



        if (performanceInfo == null)
        {
            return;
        }
        lock (performanceInfo)
        {
            string s = "";
            foreach (DictionaryEntry de in performanceInfo)
            {
                s += de.Key + "=" + de.Value + "\n";
            }
            performance.text = s;
        }

    }


    public void Connect()
    {
        
        Debug.Log("Connecting to ROS...");

        websocket = new WebSocket("ws://" + address + ":" + websocketport);
    
        websocket.OnOpen += (sender, e) => this.OnConnect();
        websocket.OnError += this.OnError;
        websocket.OnMessage += (sender, e) => this.OnMessage(e.Data);
        websocket.OnClose += (sender, e) => this.OnDisconnect();


        udpsocket = new UdpClient();

        udpsocket.Connect(address, udpPort);

        websocket.Connect();

        MessageBox.ShowMessage("正在连接功能...");
      

    }





    public void Disconnect()
    {
        

        websocket.Close();

    }





    public void OnConnect()
    {


        connected = true;


        Thread.Sleep(1500);




      
       


    }


  

   


    public void CheckConnectionAndReconn()
    {


    
        Debug.Log("Check Connection Thread ");

        if (autoReconnect == true)
        {
            
            Debug.Log("CheckConnection Reconnect");

            Connect();

        }

           


    }

   


    public void ReceiveMessage()
    {
        

        while (true)
        {



            if (udpsocket.Available > 0)
            {

               
                byte[] data = udpsocket.Receive(ref remoteEP);
                string text = Encoding.ASCII.GetString(data);
                OnMessage(text);


            }
            else
            {
                Thread.Sleep(100);
            }


        }


    }

    private void OnMessage(string s)
    {

        if (connected == false)
        {
            return;
        }


        int index = s.IndexOf(",");

        if (index > 0)
        {
            
            string key = s.Substring(0, index);

            List<string> needParselist;

               
            if (incomingMessages.ContainsKey(key))
            {
                    
                needParselist = (List<string>)incomingMessages[key];


            }
            else
            {
                 

                needParselist = new List<string>();

                incomingMessages.Add(key, needParselist);

                Thread parserThread = new Thread(new ParameterizedThreadStart(ParseMsg));

                parseThreads.Add(parserThread);

                BaseSubscriber subscriber = GetSubscriber(key);

                float minIntervel;

                minIntervel = subscriber.minProcessIntervalMillSeconds;

                parserThread.Start(new object[2]{ key, minIntervel });


            }



            lock (needParselist)
            {
                
                needParselist.Add(s);

            }

               
        }

       

    }



    public void Print(string key, int value)
    {

        lock (performanceInfo)
        {
        
            performanceInfo.Remove(key);
        
            performanceInfo.Add(key, value);
        }

    }

    public void ParseMsg(object obj)
    {

       

        object[] objs = (object[])obj;


        string msgHead = (string)objs[0];


        List<string> needParseList = (List<string>)incomingMessages[msgHead];


        float minIntervel = (float)objs[1];

        long lastParseTime = 0;

        while (true)
        {


            string msg = null;
              

            if (needParseList.Count > 0)
            {
                

                lock (needParseList)
                {

                    msg = (string)needParseList[0];

                    needParseList.RemoveAt(0);

                }


                long now = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;

                if ((now - lastParseTime) > minIntervel)
                {
               


                    JSONNode node;

                    try
                    {
                        node = JSON.Parse(msg);
                    

                        string op = node["op"];

                        if ("publish".Equals(op))
                        {
                            lock (parsedMessages)
                            {
                                parsedMessages.Add(node);

                            }
                        }
                    }
                    catch (Exception e)
                    {

                        Debug.Log(e);

                    }

                    lastParseTime = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;

                }
   


            }
            else
            {
               
                Thread.Sleep((int)minIntervel);

            }



        }

    }









    public void SendByUdp(string s)
    {


        try
        {



            byte[] data = Encoding.ASCII.GetBytes(s);

            udpsocket.Send(data, data.Length);


        }
        catch (Exception e)
        {

            Debug.LogError(e.Message);

        }

   
    }




    public void Publish(String topic, ROSBridgeMsg msg)
    {



        if (connected)
        {
     
            string s = ROSBridgeMsg.Publish(topic, msg.ToYAMLString());
       

            SendByUdp(s);

        }

    }






    public void OnError(object sender, ErrorEventArgs e)
    {


        Debug.Log("OnError:" + e.Message + " ");
       
    }


    public void OnDisconnect()
    {


        connected = false;

      
        Debug.Log("OnDisconnect");


    }



    public virtual void OnEveryUpdate()
    {



    }




    void OnConnectChange(bool isConnected)
    {


        if (isConnected)
        {


            MessageBox.ShowMessage("功能已经连接.");

            incomingMessages = new Hashtable();

            parseThreads = new List<Thread>();

            parsedMessages = new List<JSONNode>();

            performanceInfo = new Hashtable();

            receiveThread = new Thread(ReceiveMessage);

            receiveThread.Start();

            foreach (BaseSubscriber subscriber in subscribers)
            {

                string msg = ROSBridgeMsg.Subscribe(subscriber.topic, subscriber.messageType);

                if (subscriber.BigData == true)
                {

                    websocket.Send(msg);

                }
                else
                {

                    SendByUdp(msg);

                }



            }



            foreach (BasePublisher publisher in publishers)
            {

                Debug.Log("Advertise:" + ROSBridgeMsg.Advertise(publisher.topic, publisher.messageType));

                SendByUdp(ROSBridgeMsg.Advertise(publisher.topic, publisher.messageType));


            }

            whenConnected.Invoke();


           


        }
        else
        {


            MessageBox.ShowMessage("功能已经断开.");

            Clean();


            whenDisconnected.Invoke();


           

        }
       
    }

    void  Update()
    {


        OnEveryUpdate();


        if (lastConnected != connected)
        {

            OnConnectChange(connected);

            lastConnected = connected;

        }


        if (connected == false)
        {
            return;
        }




        lock (performanceInfo)
        {

            performanceInfo.Remove("UpdateParseredMessge");

            performanceInfo.Add("UpdateParseredMessge", parsedMessages.Count);


        }



        if (parsedMessages.Count > 0)
        {

            JSONNode node = null;



            lock (parsedMessages)
            {
               
                if (parsedMessages.Count > 0)
                {

                    node = parsedMessages[0];

                    parsedMessages.RemoveAt(0);


                }

            }


            if (node != null)
            {

                foreach (BaseSubscriber subscriber in subscribers)
                {

                    if (subscriber.topic.Equals(node["topic"]))
                    {
                        subscriber.AddToNeedParse(node);

                    }

                }

           
            }
		
        }


        foreach (BaseSubscriber subscriber in subscribers)
        {
            
            subscriber.OnMessage();


        }




       

        


    }


    public virtual void OnClickMap(Vector3 positionInMap)
    {


    }


   




    public BaseSubscriber GetSubscriber(string msgHead)
    {

        foreach (BaseSubscriber subscriber in subscribers)
        {

            if (subscriber.GetMsgHead().Equals(msgHead))
            {

                return subscriber;

            }


        }

        return null;
    }


    public BaseSubscriber GetSubscriber(Type subscriberType)
    {

        foreach (BaseSubscriber subscriber in subscribers)
        {

            if (subscriber.GetType() == subscriberType)
            {
                
                return subscriber;

            }


        }

        return null;
    }



    public BasePublisher GetPublisher(Type publisherType)
    {

        foreach (BasePublisher publisher in publishers)
        {

            if (publisher.GetType() == publisherType)
            {


                return publisher;
            }

        }
           

        return null;
    }

}
